/*
 * manipulator.h
 *
 *  Created on: Dec 4, 2023
 *      Author: boen
 */

#ifndef PERIPHERALS_INC_MANIPULATOR_H_
#define PERIPHERALS_INC_MANIPULATOR_H_

#include <arm_fk.h>
#include <arm_ik.h>
#include <nvic.h>

#define SERVO_MOVE_TIME_WRITE 								1
#define SERVO_MOVE_TIME_READ 								2
#define SERVO_MOVE_TIME_WAIT_WRITE 							7
#define SERVO_MOVE_TIME_WAIT_READ 							8
#define SERVO_MOVE_START 									11
#define SERVO_MOVE_STOP 									12
#define SERVO_ID_WRITE 										13
#define SERVO_ID_READ 										14
#define SERVO_ANGLE_OFFSET_ADJUST 							17
#define SERVO_ANGLE_OFFSET_WRITE 							18
#define SERVO_ANGLE_OFFSET_READ 							19
#define SERVO_ANGLE_LIMIT_WRITE 							20
#define SERVO_ANGLE_LIMIT_READ 								21
#define SERVO_VIN_LIMIT_WRITE 								22
#define SERVO_VIN_LIMIT_READ 								23
#define SERVO_TEMP_MAX_LIMIT_WRITE 							24
#define SERVO_TEMP_MAX_LIMIT_READ 							25
#define SERVO_TEMP_READ 									26
#define SERVO_VIN_READ 										27
#define SERVO_POS_READ 										28
#define SERVO_OR_MOTOR_MODE_WRITE 							29
#define SERVO_OR_MOTOR_MODE_READ 							30
#define SERVO_LOAD_OR_UNLOAD_WRITE 							31
#define SERVO_LOAD_OR_UNLOAD_READ 							32
#define SERVO_LED_CTRL_WRITE 								33
#define SERVO_LED_CTRL_READ 								34
#define SERVO_LED_ERROR_WRITE 								35
#define SERVO_LED_ERROR_READ 								36

#define SERVO_FRAME_HEADER         							0x55u
#define SERVO_TIME_OUT										1000u

#define SERVO_GRIPPER_ON									0
#define SERVO_GRIPPER_OFF									1

void serial_servo_timer(void);
int serial_servo_get_reply(uint8_t buffer[SERVO_RX_BUFF_MAX]);

int manipulator_is_ready(void);
int manipulator_set_joint(float j1, float j2, float j3, float j4);
int manipulator_set_pose(float x, float y, float z, float roll, float pitch, float yaw);
int manipulator_set_gripper(float state);
int manipulator_set_turntable(uint8_t state);

int manipulator_init(void);
int manipulator_deinit(void);
int manipulator_polling(void);


#endif /* PERIPHERALS_INC_MANIPULATOR_H_ */
